package src;

import util.RateEncoder;
import edu.wpi.first.wpilibj.*;

/**
 *
 * @author TJ
 */
public class DriveTrain {
    public static final double HIGH_INPUT_RANGE = 9;
    public static final double LOW_INPUT_RANGE = 5;

    private SpeedController leftMotors, rightMotors;
    private RobotDrive robotDrive;
    private RateEncoder leftEncoder, rightEncoder;
    private PIDController leftPID, rightPID;
    private Solenoid lowShifter, highShifter;

    private int controlMode;
    private double leftSpeed, rightSpeed;
    private boolean highGear;

    public DriveTrain() {
        leftMotors = Robot.driveTrainLeftMotors;
        rightMotors = Robot.driveTrainRightMotors;
        robotDrive = Robot.driveTrainRobotDrive;
        leftEncoder = Robot.driveTrainLeftEncoder;
        rightEncoder = Robot.driveTrainRightEncoder;
        leftPID = Robot.driveTrainLeftPID;
        rightPID = Robot.driveTrainRightPID;
        lowShifter = Robot.driveTrainLowShifter;
        highShifter = Robot.driveTrainHighShifter;

        controlMode = Robot.CONTROL_RAW;
        leftSpeed = 0.0;
        rightSpeed = 0.0;
        shiftLow();
    }

    public void setMode(int mode) {
        if(mode == Robot.CONTROL_RAW && controlMode == Robot.CONTROL_PID) {
            leftPID.disable();
            rightPID.disable();
            robotDrive.tankDrive(-leftSpeed, -rightSpeed, true);
        } else if(mode == Robot.CONTROL_PID && controlMode == Robot.CONTROL_RAW) {
            leftPID.enable();
            rightPID.enable();
        }
        controlMode = mode;
    }

    public void resetPID() {
        leftPID.reset();
        rightPID.reset();
    }

    public void set(double left, double right) {
        leftSpeed = left;
        rightSpeed = right;
        leftPID.setSetpoint(leftSpeed);
        rightPID.setSetpoint(rightSpeed);
        if(controlMode == Robot.CONTROL_RAW) {
            robotDrive.tankDrive(-leftSpeed, -rightSpeed, true);
        }
    }

    public void shiftLow() {
        highGear = false;
        leftPID.setInputRange(-LOW_INPUT_RANGE, LOW_INPUT_RANGE);
        rightPID.setInputRange(-LOW_INPUT_RANGE, LOW_INPUT_RANGE);
        lowShifter.set(true);
        highShifter.set(false);
    }

    public void shiftHigh() {
        highGear = true;
        leftPID.setInputRange(-HIGH_INPUT_RANGE, HIGH_INPUT_RANGE);
        rightPID.setInputRange(-HIGH_INPUT_RANGE, HIGH_INPUT_RANGE);
        lowShifter.set(false);
        highShifter.set(true);
    }

    public void feedDashboard() {
    }
}
